#region Using Statements
using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using KLib.Physics;
#endregion

namespace KLib.Physics.Collisions
{
    public class Arbiter
    {
        #region Fields
        private PhysicsManager physics;
        private float frictionCoEfficientCombined;

        public Geom GeometryA;
        public Geom GeometryB;
        private RigidBody bodyA;
        private RigidBody bodyB;
        internal ContactList contactList;
        private Comparison<Contact> contactComparer;
        private Contact contact;

        //TMP Vars: Declared here for GC Optimization
        float tmpFloat;
        float tmpFloat2;
        Vector2 tmpVect = Vector2.Zero;
        Vector2 vect1 = Vector2.Zero;
        Vector2 vect2 = Vector2.Zero;
        Vector2 tangent = Vector2.Zero;
        #endregion

        #region Properties

        public ContactList ContactList
        {
            get { return contactList; }
        }

        #endregion

        #region Initialization

        public Arbiter()
        {
        }

        internal Arbiter(RigidBody bodyA, RigidBody bodyB, PhysicsManager physics)
        {
            this.bodyA = bodyA;
            this.bodyB = bodyB;
            this.GeometryA = bodyA.Geometry;
            this.GeometryB = bodyB.Geometry;
            this.physics = physics;

            frictionCoEfficientCombined = (bodyA.FrictionCoefficient + bodyB.FrictionCoefficient) / 2f;

            InitializeContactList(physics.MaxContactsToDetect);
            contactComparer = new Comparison<Contact>(CompareSeparation);
        }
        #endregion

        #region Methods

        public void ConstructArbiter(RigidBody bodyA, RigidBody bodyB, PhysicsManager physics)
        {
            this.bodyA = bodyA;
            this.bodyB = bodyB;
            this.GeometryA = bodyA.Geometry;
            this.GeometryB = bodyB.Geometry;
            this.physics = physics;

            frictionCoEfficientCombined = (bodyA.FrictionCoefficient + bodyB.FrictionCoefficient) / 2f;

            InitializeContactList(physics.MaxContactsToDetect);
            contactComparer = new Comparison<Contact>(CompareSeparation);
        }

        #region PreStepImpulse()
        #region PreStepInpulse Variables
        Vector2 r1;
        Vector2 r2;
        float rn1;
        float rn2;
        float rt1;
        float rt2;
        float kTangent;
        float min;
        float restitution;
        float kNormal;
        #endregion
        internal void PreStepInpulse(float inverseDT)
        {
            if (!GeometryA.CollisionResponseEnabled || !GeometryB.CollisionResponseEnabled) return;
            for (int i = 0; i < contactList.Count; i++)
            {
                contact = contactList[i];

                //Geom Offset
                Vector2.Subtract(ref contact.Position, ref bodyA.position, out r1);
                Vector2.Subtract(ref contact.Position, ref bodyB.position, out r2);

                //Normal offset vectors
                Vector2.Dot(ref r1, ref contact.Normal, out rn1);
                Vector2.Dot(ref r2, ref contact.Normal, out rn2);

                //Mass Normal
                float inverseMassSum = bodyA.InverseMass + bodyB.InverseMass;
                Vector2.Dot(ref r1, ref r1, out tmpFloat);
                Vector2.Dot(ref r2, ref r2, out tmpFloat2);
                kNormal = inverseMassSum + bodyA.InverseMomentOfIntertia * (tmpFloat - rn1 * rn1) + bodyB.InverseMomentOfIntertia * (tmpFloat2 - rn2 * rn2);
                contact.MassNormal = 1f / kNormal;

                //MassTangent
                tmpFloat = 1f;
                Utilities.Math.Cross(ref contact.Normal, ref tmpFloat, out tangent);
                Vector2.Dot(ref r1, ref tangent, out rt1);
                Vector2.Dot(ref r2, ref tangent, out rt2);
                kTangent = bodyA.InverseMass + bodyB.InverseMass;

                Vector2.Dot(ref r1, ref r1, out tmpFloat);
                Vector2.Dot(ref r2, ref r2, out tmpFloat2);
                kTangent += bodyA.InverseMomentOfIntertia * (tmpFloat - rt1 * rt1) + bodyB.InverseMomentOfIntertia * (tmpFloat2 - rt2 * rt2);
                contact.MassTangent = 1f / kTangent;

                //Velocity Bias
                min = Math.Min(0, physics.AllowedPenitration + contact.Separation);
                contact.NormalVelocityBias = -physics.BiasFactor * inverseDT * min;

                //Restiution
                restitution = GeometryA.RestitutionCoefficient + GeometryB.RestitutionCoefficient * 0.5f;

                //Bounce Velocity
                bodyA.GetVelocityAtWorldOffset(ref r1, out vect1);
                bodyB.GetVelocityAtWorldOffset(ref r2, out vect2);
                Vector2.Subtract(ref vect2, ref vect1, out tmpVect);

                //Velocity along contact Normal
                Vector2.Dot(ref tmpVect, ref contact.Normal, out tmpFloat);
                contact.BounceVelocity = tmpFloat * restitution;

                //Apply impulses
                Vector2.Multiply(ref contact.Normal, contact.NormalImpulse, out vect1);
                Vector2.Multiply(ref tangent, contact.TangentImpulse, out vect2);
                Vector2.Add(ref vect1, ref vect2, out tmpVect);
                bodyB.ApplyImpulseAtWorldOffset(ref tmpVect, ref r2);

                Vector2.Multiply(ref tmpVect, -1, out tmpVect);
                bodyA.ApplyImpulseAtWorldOffset(ref tmpVect, ref r1);

                contact.NormalImpulseBias = 0f;
                contactList[i] = contact;
            }
        }
        #endregion

        #region ApplyImpulse()
        #region ApplyImpulse Variables
        Vector2 dv = Vector2.Zero;
        float nv;
        float normalImpulse;
        float oldImpulse;
        Vector2 impulse = Vector2.Zero;
        Vector2 impulseBias = Vector2.Zero;
        float normalVelocityBias;
        float normalImpulseBias;
        float maxTangentImpulse;
        float tangentImpulse;
        #endregion

        public void ApplyImpulse()
        {
            if (!GeometryA.CollisionResponseEnabled || !GeometryB.CollisionResponseEnabled) return;
            for (int i = 0; i < contactList.Count; i++)
            {
                contact = contactList[i];

                contact.R1.X = contact.Position.X - bodyA.Position.X;
                contact.R1.Y = contact.Position.Y - bodyA.Position.Y;

                contact.R2.X = contact.Position.X - bodyB.Position.X;
                contact.R2.Y = contact.Position.Y - bodyB.Position.Y;

                //Velocity Difference
                bodyA.GetVelocityAtWorldOffset(ref contact.R1, out vect1);
                bodyB.GetVelocityAtWorldOffset(ref contact.R2, out vect2);

                dv.X = vect2.X - vect1.X;
                dv.Y = vect2.Y - vect1.Y;

                nv = (dv.X * contact.Normal.X) + (dv.Y * contact.Normal.Y);
                normalImpulse = contact.MassNormal * -(nv + contact.BounceVelocity);

                //Clamp accumulated impulses
                oldImpulse = contact.NormalImpulse;
                contact.NormalImpulse = Math.Max(oldImpulse + normalImpulse, 0);
                normalImpulse = contact.NormalImpulse - oldImpulse;

                //Apply Contact Impulses
                impulse.X = contact.Normal.X * normalImpulse;
                impulse.Y = contact.Normal.Y * normalImpulse;

                bodyB.ApplyImpulseAtWorldOffset(ref impulse, ref contact.R2);
                impulse.X = -impulse.X;
                impulse.Y = -impulse.Y;
                bodyA.ApplyImpulseAtWorldOffset(ref impulse, ref contact.R1);

                //Velocity Bias Difference
                bodyA.GetVelocityAtWorldOffset(ref contact.R1, out vect1);
                bodyB.GetVelocityAtWorldOffset(ref contact.R2, out vect2);

                dv.X = vect2.X - vect1.X;
                dv.Y = vect2.Y - vect1.Y;

                //Velocity Bias Along Normal
                normalVelocityBias = (dv.X * contact.Normal.X) + (dv.Y * contact.Normal.Y);

                normalImpulseBias = contact.MassNormal * (-normalVelocityBias + contact.NormalVelocityBias);
                tmpFloat = contact.NormalImpulseBias;
                contact.NormalImpulseBias = Math.Max(tmpFloat + normalImpulseBias, 0);
                normalImpulseBias = contact.NormalImpulseBias - tmpFloat;

                //Apply Impulse Bias
                impulseBias.X = contact.Normal.X * normalImpulseBias;
                impulseBias.Y = contact.Normal.Y * normalImpulseBias;

                bodyB.ApplyImpulseBiasAtWorldOffset(ref impulseBias, ref contact.R2);

                impulseBias.X = impulseBias.X * -1;
                impulseBias.Y = impulseBias.Y * -1;

                bodyA.ApplyImpulseBiasAtWorldOffset(ref impulseBias, ref contact.R1);

                //Relitive Velocity at Contact
                bodyA.GetVelocityAtWorldOffset(ref contact.R1, out vect1);
                bodyB.GetVelocityAtWorldOffset(ref contact.R2, out vect2);

                dv.X = vect2.X - vect1.X;
                dv.Y = vect2.Y - vect1.Y;

                maxTangentImpulse = frictionCoEfficientCombined * contact.NormalImpulse;
                tangent.X = contact.Normal.Y;
                tangent.Y = -contact.Normal.X;

                tmpFloat = (dv.X * tangent.X) + (dv.Y * tangent.Y);

                tangentImpulse = contact.MassTangent * -tmpFloat;

                //Clamp Friction
                tmpFloat2 = contact.TangentImpulse;
                contact.TangentImpulse = MathHelper.Clamp(tmpFloat2 + tangentImpulse, -maxTangentImpulse, maxTangentImpulse);
                tangentImpulse = contact.TangentImpulse - tmpFloat2;

                impulse.X = tangent.X * tangentImpulse;
                impulse.Y = tangent.Y * tangentImpulse;

                bodyB.ApplyImpulseAtWorldOffset(ref impulse, ref contact.R2);

                impulse.X = impulse.X * -1f;
                impulse.Y = impulse.Y * -1f;

                bodyA.ApplyImpulseAtWorldOffset(ref impulse, ref contact.R1);

                contactList[i] = contact;
            }
        }
        #endregion

        public void Reset()
        {
            this.bodyA = null;
            this.bodyB = null;
            this.GeometryA = null;
            this.GeometryB = null;
            contactList.Clear();
        }

        #region Collision
        public void Collide()
        {
            contactList.Clear();
            Collide(GeometryA, GeometryB, contactList);
        }

        #region Collide Variables
        Feature feature;
        Vector2 localVert;
        Vector2 vertRef;
        Matrix tmpMatrix;
        Matrix inverseTmpMatrix;
        #endregion
        public void Collide(Geom geomA, Geom geomB, ContactList newContactList)
        {
            tmpMatrix = geomA.Matrix;
            int vertexIndex = -1;
            for (int i = 0; i < geomB.WorldVertices.Count; i++)
            {
                if (contactList.Count == physics.MaxContactsToDetect)
                    break;

                //Add Grid check here once the reformating is done of the Geom, RigidBody classes
                if (bodyA.Grid == null) break; //Grid can be null for one-way collision (point)
                vertexIndex++;
                vertRef = geomB.WorldVertices[i];

                geomA.TransformToLocalCoordinates(ref vertRef, out localVert);
            
                if(!bodyA.Grid.Intersect(ref localVert, out feature)) continue;

                if (feature.Distance < 0f)
                {
                    geomA.TransformNormalToWorld(ref feature.Normal, out feature.Normal);
                    contactList.Add(new Contact(geomB.WorldVertices[i], feature.Normal, feature.Distance));
                }
            }

            inverseTmpMatrix = Matrix.Invert(geomB.Matrix);
            tmpMatrix = geomB.Matrix;

            for (int i = 0; i < geomA.WorldVertices.Count; i++)
            {
                if (contactList.Count == physics.MaxContactsToDetect)
                    break;

                //Add Grid check here once the reformating is done of the Geom, RigidBody classes
                if (bodyB.Grid == null) break; //Grid can be null for one-way collision (point)
                vertexIndex++;
                vertRef = geomA.WorldVertices[i];

                geomB.TransformToLocalCoordinates(ref vertRef, out localVert);

                if (!bodyB.Grid.Intersect(ref localVert, out feature)) continue;

                if (feature.Distance < 0f)
                {
                    geomB.TransformNormalToWorld(ref feature.Normal, out feature.Normal);
                    feature.Normal = -feature.Normal;
                    contactList.Add(new Contact(geomA.WorldVertices[i], feature.Normal, feature.Distance));
                }
            }

            contactList.Sort(contactComparer);

            if (contactList.Count > physics.MaxContactsToDetect)
                contactList.RemoveRange(physics.MaxContactsToDetect, contactList.Count - physics.MaxContactsToDetect);

        }
        #endregion

        #endregion

        #region Helper Methods
        private void InitializeContactList(int maxContacts)
        {
            if (contactList == null) contactList = new ContactList(maxContacts);

        }
        private static int CompareSeparation(Contact contact1, Contact contact2)
        {
            if (contact1.Separation < contact2.Separation)
                return -1;
            else if (contact1.Separation == contact2.Separation)
                return 0;

            return 1;
        }
        #endregion
    }
}
